#include "Attitude_algorithm.h"

//#define PI 3.141592653589793

//tri UK = {0};//system input
//tri Xk = {0};//posterior estimation
//tri Xk_ = {0};//prior estimation
//tri Zk = {0};//measure status
//tri Pk = {1,1,1};//posterior estimation error covariance
//tri Pk_ = {0};//prior estimation error covariance
//tri K = {0};//kalman gain
//tri Q = {0.0025,0.0025,0.0025};//system noise covariance
//tri R = {0.3,0.3,0.3};//measure noise covariance
//tri euro = {0};

//////float mx_max = FLT_MIN,mx_min = FLT_MAX;
////float mx_max = 30.0f,mx_min = -24.9f;
//////Maximum and minimum values of magnetometer X-axis
//////float my_max = FLT_MIN,my_min = FLT_MAX;
////float my_max = 25.2f,my_min = -30.15f;
//////Maximum and minimum values of magnetometer Y-axis
//////float mz_max = FLT_MIN,mz_min = FLT_MAX;
////float mz_max = 33.3f,mz_min = -23.25f;

//static float T = 0.01f;

//void kalman_filter_solution(MPU *mpu)
//{
//    UK.x = mpu->gx + sin(Xk.x) * tan(Xk.y) * mpu->gy + cos(Xk.x) * tan(Xk.y) * mpu->gz;
//    UK.y = cos(Xk.x) * mpu->gy - sin(Xk.x) * mpu->gz;
//    UK.z = sin(Xk.x) * mpu->gy / cos(Xk.y) + cos(Xk.x) * mpu->gz / cos(Xk.y);
//    //step 1 system input
//    Xk_.x = Xk.x + T * UK.x;
//    Xk_.y = Xk.y + T * UK.y;
//    Xk_.z = Xk.z + T * UK.z;

//    if (Xk_.z > PI)
//    {
//        Xk_.z -= -PI*2;
//    }else if (Xk_.z < - PI)
//    {
//        Xk_.z += PI*2;
//    }

//    //step2 prior estimation

//    Pk_.x = Pk.x + Q.x;
//    Pk_.y = Pk.y + Q.y;
//    Pk_.z = Pk.z + Q.z;

//    //step3 prior estimation error covariance

//    K.x = Pk_.x / (Pk_.x + R.x);
//    K.y = Pk_.y / (Pk_.y + R.y);
//    K.z = Pk_.z / (Pk_.z + R.z);

//    //step4 kalman gain

//    // float mbx,mby,mbz;
//    // float mZx,mZy,mZz;

//    // mbx = (mx - (mx_max + mx_min)/2.f) / ((mx_max - mx_min)/2.f);
//	// mby = (my - (my_max + my_min)/2.f) / ((my_max - my_min)/2.f);
//	// mbz = (mz - (mz_max + mz_min)/2.f) / ((mz_max - mz_min)/2.f);

//    // mZx = cos(Xk[1]) * mbx + sin(Xk[1]) * sin(Xk[0]) * mby + sin(Xk[1]) * cos(Xk[0]) * mbz;
//	// mZy = cos(Xk[0]) * mby - sin(Xk[0]) * mbz;
//	// mZz = -sin(Xk[1]) * mbx + cos(Xk[1]) * sin(Xk[0]) * mby + cos(Xk[1]) * cos(Xk[0]) * mbz;

//    //step5 calculation yaw by magnetometer data

//    Zk.x = atan(mpu->ay/mpu->az);
//    Zk.y = -atan(mpu->ax/(sqrt(mpu->ay*mpu->ay + mpu->az*mpu->az)));
//    // Zk.z = atan2(mZy,mZx);

//		Xk.x = (1 - K.x ) * Xk_.x + K.x * Zk.x;
//		Xk.y = (1 - K.y) * Xk_.y + K.y * Zk.y;
////		Xk.z = (1 - K.z) * Xk_.z + K.z * Zk.z;
//    //step6 measure data

//    Pk.x = (1-K.x) * Pk_.x;
//    Pk.y = (1-K.y) * Pk_.y;
////    Pk.z = (1-K.z) * Pk_.z;

//    //step7 posterior estimation error covariance

//    euro.x = Xk.x / PI * 180.f;
//    euro.y = Xk.y / PI * 180.f;
////    euro.z = Xk.z / PI * 180.f;

//    //step8 calculate the angle ,unit:degree
//}

